;; Non-robust computational geometry functions
;; Copyright (C) 2021  Rocks Mazama
;;
;; This program is free software: you can redistribute it and/or modify
;; it under the terms of the GNU General Public License as published by
;; the Free Software Foundation, either version 3 of the License, or
;; (at your option) any later version.
;;
;; This program is distributed in the hope that it will be useful,
;; but WITHOUT ANY WARRANTY; without even the implied warranty of
;; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
;; GNU General Public License for more details.
;;
;; You should have received a copy of the GNU General Public License
;; along with this program.  If not, see <https://www.gnu.org/licenses/>.

(ns mazama.compgeom.spline
  (:require
   [mazama.compgeom.core :refer [v+ v- v*]]
   [mazama.linalg.sole :as sole]
   [mazama.linalg.tridiagonal :refer [tridiagonal]]))

(defn- spline-helper
  "Handle the cases when there are fewer than three points.
  Otherwise call the given function to compute the spline."
  [P f]
  (let [n (count P)]
    (case n
      0 P
      1 [[(first P) (first P) (first P) (first P)]]
      2 [[(first P) (first P) (second P) (second P)]]
      (f P))))

(defn- b-spline-C0
  "Compute the first Bezier control point for each segment."
  [B]
  (mapv #(v+ (v* 2/3 %1) (v* 1/3 %2)) B (rest B)))

(defn- b-spline-C1
  "Compute the second Bezier control point for each segment."
  [B]
  (mapv #(v+ (v* 1/3 %1) (v* 2/3 %2)) B (rest B)))

(defn- b-spline-P
  "Compute the Bezier end point corresponding to each B-spline control point."
  [C1 C0]
  (mapv #(v* 1/2 (v+ %1 %2)) C1 C0))

;;; Reference:
;;; http://mirror.las.iastate.edu/tex-archive/graphics/pstricks/contrib/pst-bspline/pst-bspline-doc.pdf, Section 3
(defn- closed-uniform-cubic-b-spline-function
  "Assumes B contains three or more points."
  [B]
  (let [B' (conj (vec B) (first B))
        C0 (b-spline-C0 B')
        C1 (b-spline-C1 B')
        C1' (cons (peek C1) C1)
        P (b-spline-P C1' C0)
        P' (conj P (first P))]
    (map vector P' C0 C1 (rest P'))))

(defn closed-uniform-cubic-b-spline
  "Compute the closed uniform cubic b-spline from the given set of control points.
  The returned value is a sequence of vectors, one for each cubic bezier
  segment. Each vector consists of four points: S0, C0, C1, S1, where
  S0 and S1 are the endpoints of the segment, and C0 and C1 are the
  control points."
  [B]
  (spline-helper B closed-uniform-cubic-b-spline-function))

;;; Reference:
;;; https://www.math.ucla.edu/~baker/149/handouts/y_splines.pdf, Section 4.
(defn- open-relaxed-uniform-cubic-b-spline-function
  "Assumes B contains three or more points."
  [B]
  (let [B (vec B)
        C0 (b-spline-C0 B)
        C1 (b-spline-C1 B)
        P (b-spline-P C1 (rest C0))
        P (cons (first B) (conj P (peek B)))]
    (map vector P C0 C1 (rest P))))

(defn open-relaxed-uniform-cubic-b-spline
  "Compute the open relaxed uniform cubic b-spline from the given set of control points.
  The returned value is a sequence of vectors, one for each cubic bezier
  segment. Each vector consists of four points: S0, C0, C1, S1, where
  S0 and S1 are the endpoints of the segment, and C0 and C1 are the
  control points."
  [B]
  (spline-helper B open-relaxed-uniform-cubic-b-spline-function))

(defn- closed-bezier-b
  "Return the b for a closed bezier curve, Ax=b"
  [P]
  (let [L (mapv #(v* 4 %) P)
        R (mapv #(v* 2 %) (conj (vec (rest P)) (first P)))]
    {:ncols 2 :data (flatten (mapv v+ L R))}))

(defn- closed-bezier-A
  "Return the A for a closed bezier curve, Ax=b"
  [n]
  (letfn [(first-row [] (conj (into [4 1] (repeat (- n 3) 0))
                              1))
          (ith-row [i] (into (vec (repeat (dec i) 0))
                             (into [1 4 1] (repeat (- n i 2) 0))))
          (last-row [] (into (into [1] (repeat (- n 3) 0))
                             [1 4]))]
    (loop [result []
           i 0]
      (cond
        (= i n) {:ncols n :data result}
        (zero? i) (recur (into result (first-row)) (inc i))
        (= i (dec n)) (recur (into result (last-row)) (inc i))
        :else (recur (into result (ith-row i)) (inc i))))))

(defn- closed-bezier-C0
  "Compute the first control point for each segment."
  [P]
  (let [n (count P)
        A (closed-bezier-A n)
        b (closed-bezier-b P)
        {:keys [ncols data]} (sole/solve A b)]
    (map vec (partition ncols data))))

(defn- closed-bezier-C1
  "Compute the second control point for each segment."
  [P C0]
  (letfn [(b [p a] (v- (v* 2 p) a))]
    (conj (mapv b (rest P) (rest C0))
          (b (first P) (first C0)))))

(defn- closed-cubic-bezier-spline-function
  "Compute the closed cubic bezier piecewise interpolation curve.
  Assumes P contains three or more points."
  [P]
  (let [C0 (closed-bezier-C0 P)
        C1 (closed-bezier-C1 P C0)]
    (map vector
         P
         C0
         C1
         (conj (vec (rest P)) (first P)))))

(defn closed-cubic-bezier-spline
  "Compute the closed cubic bezier piecewise curve that interpolates
  the sequence of given data points.
  It is assumed that the input data points are implicitly closed, i.e.
  the last point should *not* be the same as the first point.
  The returned value is a sequence of vectors, one for each bezier segment.
  Each vector consists of four points: S0, C0, C1, S1, where S0 and S1 are
  the endpoints of the segment, and C0 and C1 are the control points."
  [P]
  (spline-helper P closed-cubic-bezier-spline-function))

(defn- open-bezier-d [P]
  (loop [result (vector (v+ (first P) (v* 2 (second P))))
         left (rest P)
         right (rest left)]
    (if (empty? (rest right))
      (conj result (v+ (v* 8 (first left)) (first right)))
      (let [d (v+ (v* 4 (first left)) (v* 2 (first right)))]
        (recur (conj result d) (rest left) (rest right))))))

(defn- open-bezier-C0 [P]
  (let [n (dec (count P)) ;n is the number of segments, not the number of points
        a (conj (vec (repeat (- n 2) 1)) 2)
        b (conj (into [2] (repeat (- n 2) 4)) 7)
        c (repeat (dec n) 1)
        d (open-bezier-d P)]
    (map vector
         (tridiagonal a b c (map first d))
         (tridiagonal a b c (map second d)))))

(defn- open-bezier-C1 [P C0]
  (letfn [(b [p a] (v- (v* 2 p) a))]
    (loop [result []
           P (rest P)
           C0 (rest C0)
           a (first C0)]
      (if (empty? C0)
        (conj result (v* 1/2 (v+ (first P) a)))
        (let [a (first C0)]
          (recur (conj result (b (first P) a)) (rest P) (rest C0) a))))))

;;; Reference:
;;; https://www.michael-joost.de/bezierfit.pdf
(defn- open-relaxed-cubic-bezier-spline-function
  "Assumes P contains three or more points."
  [P]
  (let [C0 (open-bezier-C0 P)
        C1 (open-bezier-C1 P C0)]
    (map vector P C0 C1 (rest P))))

(defn open-relaxed-cubic-bezier-spline
  "Compute the open relaxed cubic bezier piecewise curve that interpolates
  the sequence of given data points.
  The returned value is a sequence of vectors, one for each bezier segment.
  Each vector consists of four points: S0, C0, C1, S1, where S0 and S1 are
  the endpoints of the segment, and C0 and C1 are the control points."
  [P]
  (spline-helper P open-relaxed-cubic-bezier-spline-function))
